#define BOOST_TEST_MODULE MMMTools_MMMLegacyMotionViewer_SetupTest
#include <Test.h>

#ifdef WIN32
#define _CRTDBG_MAP_ALLOC
#include <stdlib.h>
#include <crtdbg.h>
#endif 

#include <MMMSimoxTools/MMMSimoxTools.h>
#include <VirtualRobot/RobotNodeSet.h>

#include "../MMMLegacyMotionViewerConfiguration.h"
#include <boost/test/unit_test.hpp>

#include <MMM/Motion/Motion.h>
#include <MMM/Model/Model.h>

#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Model/ModelProcessorWinter.h>
#include <MMM/Motion/MotionReaderC3D.h>
#include <MMM/Motion/MotionReaderXML.h>
#include <MMM/Motion/MarkerMotion.h>
#include <MMM/Motion/MarkerData.h>
#include <MMM/Model/ModelProcessorFactory.h>
#include <MMM/Motion/Legacy/Converter/ConverterFactory.h>
#include <MMM/Motion/Legacy/Converter/MarkerBasedConverter.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

// BOOST_AUTO_TEST_SUITE(XMLTest)

std::vector <MMM::MotionPtr> loadMotion(std::string filename)
{
    MMM::MotionReaderXMLPtr r(new MMM::MotionReaderXML());
    std::vector < std::string > motionNames = r->getMotionNames(filename);
    std::vector <MMM::MotionPtr> motions;
    BOOST_CHECK(motionNames.size()>0);

    int frameCount = -1;
    for(size_t i = 0; i < motionNames.size(); i++)
    {
        MMM::MotionPtr motion = r->loadMotion(filename,motionNames[i]);
        BOOST_CHECK(motion);
        BOOST_CHECK(frameCount == -1 || frameCount == motion->getNumFrames());
        frameCount = motion->getNumFrames();

        BOOST_CHECK(motion->getModel());

        VirtualRobot::RobotPtr robot = MMM::SimoxTools::buildModel(motion->getModel());
        BOOST_CHECK(robot);
        BOOST_CHECK(robot->getRobotNodes().size()>2);

        std::vector<std::string> jointNames = motion->getJointNames();
        if (jointNames.size()>0)
        {
            std::string rnsName("MMMLegacyMotionViewerRNS");
            if (robot->hasRobotNodeSet(rnsName))
                robot->deregisterRobotNodeSet(robot->getRobotNodeSet(rnsName));
            VirtualRobot::RobotNodeSetPtr rns = VirtualRobot::RobotNodeSet::createRobotNodeSet(robot, rnsName, jointNames, "", "", true);
            BOOST_CHECK(rns);
        }
        motions.push_back(motion);
    }
    return motions;
}

BOOST_AUTO_TEST_CASE(testLoadMotion)
{
    MMMLegacyMotionViewerConfiguration c;
    std::vector <MMM::MotionPtr> motions = loadMotion(c.motionFile);
    BOOST_CHECK(motions.size()>0);
}

BOOST_AUTO_TEST_CASE(testApplyMotion)
{
    MMMLegacyMotionViewerConfiguration c;
    std::vector <MMM::MotionPtr> motions = loadMotion(c.motionFile);
    BOOST_CHECK(motions.size()>0);
    MMM::MotionPtr motion = motions[0];
    VirtualRobot::RobotPtr robot = MMM::SimoxTools::buildModel(motion->getModel());
    BOOST_CHECK(robot);
    std::string rnsName("MMMLegacyMotionViewerRNS");
    std::vector<std::string> jointNames = motion->getJointNames();
    if (robot->hasRobotNodeSet(rnsName))
        robot->deregisterRobotNodeSet(robot->getRobotNodeSet(rnsName));
    VirtualRobot::RobotNodeSetPtr rns = VirtualRobot::RobotNodeSet::createRobotNodeSet(robot, rnsName, jointNames, "", "", true);
    BOOST_CHECK(rns);

    for (unsigned int i=0;i<motion->getNumFrames();i++)
    {
        MMM::MotionFramePtr md = motion->getMotionFrame(i);
        BOOST_CHECK(md);
        robot->setGlobalPose(md->getRootPose());
        BOOST_CHECK(md->joint.rows() == rns->getSize());
        rns->setJointValues(md->joint);
    }
}
// BOOST_AUTO_TEST_SUITE_END()
